import { Message } from 'element-ui'
import store from '../store/index'
import { DATA } from './data'
import { NAV, PARAMS, checkStr, withNs, isNameUsedWaypoints, isNameUsedtraject, getTrajectoryByName, verifyNavCtrlStatus } from './navigation'
import { TF, quaternionToYaw } from './tf';
import { UI } from './ui'
import { getWaypointByName } from './init'

/**
 * 急停
 */
export function stopRobot(){
    NAV.navCtrl('', 0);
};

/**
 * 点击新建地图
 * seq=1
 * frame_id=salm
 * **/
export function newMapping(){
    if(DATA.rosMode === 'gmapping'){
        Message({
            showClose: true,
            message: '当前状态为建图状态，请点击保存',
            type: 'error'
        });
        return;
    }
    verifyNavCtrlStatus();
    DATA.loading = '新建中';
    //发布启动建图话题,开始打点
    let message = {
        seq: 1,
        frame_id: 'slam'
    }
    var startMappingTopic = new ROSLIB.Topic({
        ros: NAV.ros,
        name: withNs('/task_switch'),
        messageType: 'std_msgs/Header'
    });
    var startMessage = new ROSLIB.Message(message);
    startMappingTopic.publish(startMessage);
    console.log('[INFO]Start mapping');
    setTimeout(function(){
        //隐藏添加地图模态框，显示保存地图
        store.commit('switchEditMapBtns',true);
        DATA.loading = false;
    },2000)
};

/**
 * 点击继续建图
 * seq=2
 * frame_id=salm
 * **/
export function continueMapping(){
    if(DATA.rosMode === 'gmapping'){
        Message({
            showClose: true,
            message: '当前状态为建图状态，请点击保存',
            type: 'error'
        });
        return;
    }
    verifyNavCtrlStatus();
    DATA.loading = '加载中';
    //发布启动建图话题,开始打点
    let goonMsg = {
        seq: 2,
        frame_id: 'slam'
    }
    var goonMappingTopic = new ROSLIB.Topic({
        ros: NAV.ros,
        name: withNs('/task_switch'),
        messageType: 'std_msgs/Header'
    });
    var goonMessage = new ROSLIB.Message(goonMsg);
    goonMappingTopic.publish(goonMessage);
    console.log('[INFO]continue mapping');
    setTimeout(function(){
        //显示保存地图按钮
        store.commit('switchEditMapBtns',true);
        DATA.loading = false;
    },2000)
    
};

/**
 * 保存地图
 * 保存：seq=1,frame_id=map
 * 关闭建图:seq=0,frame_id=slam
 * **/
export function submitSaveMapBtn(){
    DATA.loading = '保存中';
    //保存地图
    let saveMsg = {
        seq: 1,
        frame_id: 'save_map'
    }
    var startMappingTopic = new ROSLIB.Topic({
        ros: NAV.ros,
        name: withNs('/task_switch'),
        messageType: 'std_msgs/Header'
    });
    var saveMessage = new ROSLIB.Message(saveMsg);
    startMappingTopic.publish(saveMessage);
    
    //一秒之后关闭建图
    setTimeout(function(){
        let closeMsg = {
            seq: 0,
            frame_id: 'slam'
        }
        var closeMappingTopic = new ROSLIB.Topic({
            ros: NAV.ros,
            name: withNs('/task_switch'),
            messageType: 'std_msgs/Header'
        });
        var closeMessage = new ROSLIB.Message(closeMsg);
        closeMappingTopic.publish(closeMessage);
    },1000);
    setTimeout(function(){
        //关闭保存地图按钮
        store.commit('switchEditMapBtns',false);
        DATA.loading = false;
        Message({
            showClose: true,
            message: '保存成功',
            type: 'success'
        });
    },5000)
};

/**
 * 提交授权密码
 * 验证成功，授权缓存authorizationPassword
 * 通过检测缓存authorizationPassword来判断是否已授权
 * **/
export function submitAuthor(authorPassword){
    if(authorPassword === store.state.authorizationPassword){
        store.commit('switchAutherModule',false);
        var storage = window.localStorage;
        storage.setItem('authorizationPassword',authorPassword);
    }else{
        Message({
            showClose: true,
            message: '密码错误，请重新输入',
            type: 'error'
        });
        var storage = window.localStorage;
        storage.removeItem('authorizationPassword');
    }
};

/**
 * 订阅电量
 *  **/
export function subButtery(){
    var waypointUserSubTopic = new ROSLIB.Topic({
        ros: NAV.ros,
        name:withNs('/battery'),
        messageType: 'sensor_msgs/BatteryState'
    })
    
    waypointUserSubTopic.subscribe(function(msg){
        if(!msg.percentage){
            return;
        }
        let butteryNum = Math.round(msg.percentage*100);
        if(butteryNum <= 15){
            store.commit('setBatteryBg',"#F56C6C")
        }
        store.commit('subButtery',butteryNum);
    })
};

/**
 *点击新建地图
 *输入地图名称后
 * */
export function addNewMap(mapName){
    let repeatName = DATA.mapList.indexOf(mapName);
    if(repeatName !== -1){
        Message({
            showClose: true,
            message: '地图名称已存在，请重新命名',
            type: 'error',
            duration: 3000
        });
        return;
    }else{
        if (DATA.rosMode === NAV.CmdEnum.Navigation){
            let input = mapName;
            //判断是否有效
            if (!checkStr(input)){
                Message({
                    showClose: true,
                    message: '无效的地图名称',
                    type: 'error',
                    duration: 3000
                });
                return;
            }

            DATA.loading = '新建中';
            //发布启动建图话题,开始打点
            let message = {
                seq: NAV.topicValues.seq1,
                frame_id: NAV.topicValues.frameIdSlam
            }
            NAV.topicPublish(NAV.topicNames.taskSwitch, NAV.topicMessageType.stdMsgsHeader, message);
            console.log('[INFO]Start mapping');
            //隐藏添加地图模态框，显示保存地图
            store.commit('switchAddMap',false);
            store.commit('switchEditMapBtns',true);
            DATA.loading = false;
        }
    }
};


//选择地图
export function changeMap(mapName){
    DATA.loading = '切换中';
    try{
        NAV.pubCmdString(NAV.CmdEnum.MapUpdate + ':' + mapName, true);
    }
    catch(err){
        Message({
            showClose: true,
            message: `切换地图失败${err}`,
            type:'error',
            duration: 3000
        });
    }
};

//删除地图
export function DeleteMap(mapName){
	if (mapName === 'master')
	{
        Message({
            showClose: true,
            message: `不允许删除master版本地图`,
            type:'error',
            duration: 3000
        });
		return;
    }
    DATA.loading = '删除中';
    try{
        NAV.pubCmdString(NAV.CmdEnum.MapDelete + ':' + mapName);
        DATA.loading = false;
    }
    catch(err){
        DATA.loading = false;
        Message({
            showClose: true,
            message: `删除地图失败${err}`,
            type:'error',
            duration: 3000
        });
    }
	
};


/**
 * 机器人定位
 * **/
export function locationRobot(status){
    console.log(status)
    DATA.display = {
        property: 'robotPose',
        value: status
    };
};


/**
 * 显示/隐藏站点列表
 * 先判断有无站点数据
 * 发送站点话题，显示渲染站点
 *  **/
export function showStations(status){
    if (!DATA.waypointsMsg) {
        Message({
            showClose: true,
            message: '站点不可用',
            type: 'error',
            duration: 0
        });
        return;
    }
    try{
        DATA.display = {
            property: 'waypoints',
            value: status
        };
    }catch(err){
        console.log("ros连接失败，无法使用站点")
    }
};

/**
 * 点击添加站点
 * 显示/隐藏 站点表单和站点列表
 * 计算小车定位和姿态
 * 缓存小车姿态
 * **/
export function clickAddStation(){
    //判断机器人姿态
    if(DATA.robotPoseMsg){
        //计算修改小车坐标
        let yawRad = quaternionToYaw(DATA.robotPoseMsg.pose.pose.orientation);
        let yawDeg = (yawRad * 180 / Math.PI).toFixed(8);
        let infos = {
            positionX:DATA.robotPoseMsg.pose.pose.position.x.toFixed(5),
            positionY:DATA.robotPoseMsg.pose.pose.position.y.toFixed(5),
            positionZ:DATA.robotPoseMsg.pose.pose.position.z.toFixed(5),
            orientationX:DATA.robotPoseMsg.pose.pose.orientation.x.toFixed(5),
            orientationY:DATA.robotPoseMsg.pose.pose.orientation.y.toFixed(5),
            orientationZ:DATA.robotPoseMsg.pose.pose.orientation.z.toFixed(5),
            orientationW:DATA.robotPoseMsg.pose.pose.orientation.w.toFixed(5),
            yaw:yawDeg
        }
        store.commit('setRobotPosition',infos)
    }else{
        Message({
            showClose: true,
            message: '无法获取当前机器人姿态',
            type: 'error',
            duration: 3000
        });
        return;
    }
    //隐藏站点列表
    store.commit('switchStationList',false);
    //显示添加站点
    store.commit('switchAddStation',true);
};

/**
 * 新建站点-提交
 * stationName:站点名称，stationType：站点类型，frameId
 * closeEnough：范围，timeout：超时，stationMode：站点模式
 * **/
export function submitAddStation(infos){
    console.log(infos);
    let stationName = infos.stationName;
    let stationType = infos.type;
    let frameId = infos.frameId;
    let closeEnough = parseFloat(infos.scope);
    let timeout = parseFloat(infos.timeout);
    let stationMode = infos.mode;
    if(!checkStr(stationName)){
        Message({
            showClose: true,
            message: '站点名称不合法',
            type: 'error',
            duration: 3000
        });
        return;
    }
    if(isNameUsedWaypoints(stationName)){
        Message({
            showClose: true,
            message: '该站点名称已存在',
            type: 'error',
            duration: 3000
        });
        return;
    }

    if (!frameId){
        if (stationType === 'initial_pose' || stationType === 'goal'){
            Message({
                showClose: true,
                message: '请输入frame_id',
                type: 'error',
                duration: 3000
            });
            return;
        }
    }

    //如果是声音播放类型，切换站点模式
    if (stationType === 'sound_play'){
        stationMode = infos.voiceMode;
    }

    //如果是call类型，切换站点模式
    if(stationMode == 'call'){
        stationMode = infos.callMode;
    }

    //如果是goto类型，切换站点模式
    if(stationMode == 'goto'){
        stationMode = infos.gotoMode;
    }

    //判断站点模式是否为空
    if(!checkStr(stationMode)){
        Message({
            showClose: true,
            message: '站点模式不合法',
            type: 'error',
            duration: 3000
        });
        return;
    }
    
    let pose = {
        // position: {
        //     x: infos.positionX,
        //     y: infos.positionY,
        //     z: infos.positionZ,
        // },
        
        // orientation: {
        //     x: infos.orientationX,
        //     y: infos.orientationY,
        //     z: infos.orientationZ,
        //     w: infos.orientationW,
        // }
        position: DATA.robotPoseMsg.pose.pose.position,
        orientation: DATA.robotPoseMsg.pose.pose.orientation
    }; 

    let waypointInfo = {
        stationName,
        stationType,
        stationMode,
        frameId,
        closeEnough,
        timeout,
        pose
    }
    NAV.addWaypoint(waypointInfo);
    hideOtherModule();
};

/**
 * 点击站点图标或名字
 * 显示站点详情、遮罩，隐藏站点列表
 * **/
export function showStationDetail(fullName){
    var waypoint = getWaypointByName(fullName);//站点信息
	var wpInfo = JSON.parse(waypoint.header.frame_id);
	if (waypoint){
        hideOtherModule();
        store.commit('switchShowMask',true);
        store.commit('switchStationModule', true);
        store.commit('setStationDetail',JSON.stringify(waypoint));
        store.commit('setStationName',fullName);//缓存站点名称
        store.commit('setStationType',wpInfo.type);//类型
        store.commit('setStationMode',wpInfo.failure_mode);//模式
        store.commit('setStationEnough',wpInfo.close_enough);//站点范围
        store.commit('setStationEndtime',wpInfo.goal_timeout);//超时时间
		console.log(`[INFO]waypoint:\n-type: ${wpInfo.type};\n-name: ${wpInfo.name}.`);
	}else{
        Message({
            showClose: true,
            message: `没有找到${fullName}站点`,
            type: 'error',
            duration: 3000
        });
		console.warn(`[WARN]Can not find waypoint ${fullName}`);
	}    
};

/**
 * 导航到指定站点
 * **/
export function navNewStation(fullName){
	NAV.navCtrl(fullName, 1);
};

/**
 * 点击删除站点按钮
 * 显示删除模态框
 * 如果导航中 return
 * 0和4是空闲状态
 * **/
export function delStationBtn(fullName){
	verifyNavCtrlStatus();
    hideOtherModule();
    store.commit('switchShowMask',true);
    store.commit('switchDeleteStation',true);
};

/**
 * 确定删除站点topic
 * **/
export function delStation(fullName){
    NAV.delWaypoint(fullName);
};

/**
 * 显示/隐藏轨迹模态框
 * **/
export function showTrack(status){
    if (!DATA.trajectoriesMsg){
        Message({
            showClose: true,
            message: '轨迹不可用',
            type: 'error',
            duration: 3000
        });
        return;
    }
    DATA.isDock = status;
};

/**
 * 点击轨迹显示轨迹详情
 * 设置轨迹详情和轨迹名称
 * **/
export function showTrackDetail(name){
    hideOtherModule();
    store.commit('switchShowMask',true);
    store.commit('switchTrackModule', true);
    var tracks = getTrajectoryByName(name);
    if(tracks.waypoints){
        store.commit("setTrackDetail",tracks.waypoints);
    }
    console.log(tracks)
    store.commit('setTrackName',name);
};

/**
 * 新建轨迹
 * 提交
 * **/
export function submitAddTrack(infos){
    console.log(DATA.waypointsMsg)
    if(infos.name === ""){
       Message({
            showClose: true,
            message: '请完善轨迹名称',
            type: 'warning'
        });
        return
    }
    if(infos.stations.length === 0){
        Message({
            showClose: true,
            message: '请至少选择一个站点',
            type: 'warning'
        });
        return
    }
    if (isNameUsedtraject(infos.name)){
        Message({
            showClose: true,
            message: '该轨迹名称已存在',
            type: 'warning'
        });
        return
    }
    NAV.addTrajectory(infos);
    hideOtherModule();
}

/**
 * 点击导航指定轨迹
 * **/
export function navTrack(name){
	var trajInfo = getTrajectoryByName(name);
	NAV.navCtrl(trajInfo.name, 1);
};

/**
 * 点击删除轨迹按钮
 * 显示模态框
 * 如果导航中 return
 * **/
export function deleteTrack(name){
    verifyNavCtrlStatus();
    hideOtherModule();
    store.commit('switchShowMask',true);
    store.commit('switchDeleteTrack',true);
};

/**
 * 确定删除轨迹
 * **/
export function deltrack(fullName){
    NAV.delTrajectory(fullName);
}


/**
 * 手势控制上下左右
 * **/
export function dragControl(direction){
    switch(direction){
        case 'up':
            NAV.manualCtrl({
                linear: NAV.ManualCtrlVel.linear 
            });
            break;

        case 'down':
            NAV.manualCtrl({
                linear: -NAV.ManualCtrlVel.linear 
            });
            break;

        case 'left':
            NAV.manualCtrl({
                angular: NAV.ManualCtrlVel.angular 
            });
            break;

        case 'right':
            NAV.manualCtrl({
                angular: -NAV.ManualCtrlVel.angular 
            });
            break;
        default:
            break;
    }
};

/**
 * 控制放大
 *  一秒切换logo过度效果
 * **/
export function zoomIn(scale){
    console.log(DATA.robotPoseStage)
    console.log(DATA.stage)
    console.log(DATA.mapScaleStage)
    if(!DATA.mapScaleStage){
        return
    }
    
    if (DATA.stage.stage.scaleX > 6 * DATA.mapScaleStage.x 
        || DATA.stage.stage.scaleY > 6 * DATA.mapScaleStage.y)
    {
        console.log('[INFO]Max scale');
        return;
    }

    store.commit('checkLogo',false)
    setTimeout(function(){
        store.commit('checkLogo',true)
    },1000);

    // scale bitmap
    DATA.stage.stage.scaleX *= scale;
    DATA.stage.stage.scaleY *= scale;

    let mapWidth = TF.pxToRos({x:DATA.mapMsg.info.width,y:DATA.mapMsg.info.height})
    // store.commit('mapScale',Math.round(mapWidth.x/DATA.stage.stage.scaleX)/10)
    store.commit('mapScale',Math.round(DATA.stage.stage.scaleX))

    //总宽高
    var width = DATA.stage.stage.scaleX * DATA.stage.width;
    var height = DATA.stage.stage.scaleY * DATA.stage.height;
    DATA.stage.stage.regX = DATA.robotPoseStage.x - DATA.stage.width/2
    DATA.stage.stage.regY = + DATA.robotPoseStage.y - DATA.stage.height/2
    // DATA.stage.stage.regX = Math.round(0.5 * (width - DATA.stage.width * DATA.stage.x));
    // DATA.stage.stage.regY = Math.round(0.2 * (height - DATA.stage.height * DATA.stage.y));
    
};

/**
 * 控制缩小
 * 一秒切换logo过度效果
 * **/
export function zoomOut(scale){
    if(!DATA.mapScaleStage){
        return
    }
    if (DATA.stage.stage.scaleX < DATA.mapScaleStage.x/6
        || DATA.stage.stage.scaleY < DATA.mapScaleStage.y/6)
    {
        console.log('[INFO]Max scale');
        return;
    }

    store.commit('checkLogo',false)
    setTimeout(function(){
        store.commit('checkLogo',true)
    },1000);

    // scale bitmap
    DATA.stage.stage.scaleX /= scale;
    DATA.stage.stage.scaleY /= scale;

    let mapWidth = TF.pxToRos({x:DATA.mapMsg.info.width,y:DATA.mapMsg.info.height})
    // store.commit('mapScale',Math.round(mapWidth.x/DATA.stage.stage.scaleX)/10)
    store.commit('mapScale',Math.round(DATA.stage.stage.scaleX))

    //总宽高
    var width = DATA.stage.stage.scaleX * DATA.stage.width;
    var height = DATA.stage.stage.scaleY * DATA.stage.height;
    DATA.stage.stage.regX = DATA.robotPoseStage.x - DATA.stage.width/2
    DATA.stage.stage.regY = DATA.robotPoseStage.y -  DATA.stage.height/2
    // DATA.stage.stage.regX = Math.round(0.5 * (width - DATA.stage.width * DATA.stage.x));
    // DATA.stage.stage.regY = Math.round(0.2 * (height - DATA.stage.height * DATA.stage.y));
};


/**
 * 显示隐藏地图，激光，路径轮廓
 * type:类型
 * flag:true ? false
 * **/
export function showMapFunction(type, flag){
    DATA.display = {
        property: type,
        value: flag
    };
};



/**
 * 位置估计
 * **/
export function locationEstimate(type){
    if(type){
        UI.poseEstimate();
        console.log(type)
    }
};

/**
 * 点击PLC
 * **/
export function showPlc(){

};

/**
 * 调节声音
 * **/
export function adjustVoice(num){
    var voiceTopic = new ROSLIB.Topic({
        ros: NAV.ros,
        name: withNs('/robotsound'),
        messageType: 'sound_play/SoundRequest'
    });
    DATA.topic['voiceTopic'] = voiceTopic;
    var msg = new ROSLIB.Message({
        sound: -2,
        command: 2,
        volume: num,
        arg: '/home/hitrobot/workspaces/hitrobot/release/devel/share/bringup/param/sound.ogg',
        arg2: ''
    });
    voiceTopic.publish(msg);
};


/**
 * 提交更新
 * **/
export function submitupdate(type,info){
    NAV.updateSystem(type, info);
};


/**
 * 提交WiFi设置
 * **/
export function subWifiSet(options){
    let ssid = options.ssid;
    let password = options.password;
    let ip = options.ip;
    let mask = options.mask;
    let geteway = options.gateway;
    let checked = options.checked;
    if(!ssid || !password){
        Message({
            showClose: true,
            message: 'wifi名称或密码不合法',
            type: 'error',
            duration: 3000
        });
        return;
    }
    
    if (!checkIp(ip)){
        Message({
            showClose: true,
            message: 'ip不合法',
            type: 'error',
            duration: 3000
        });
        return;
    }

    if (mask === ''){
        Message({
            showClose: true,
            message: '子掩码不合法',
            type: 'error',
            duration: 3000
        });
        return;
    }

    if (geteway === ''){
        Message({
            showClose: true,
            message: '网关不合法',
            type: 'error',
            duration: 3000
        });
        return;
    }

    //十进制转换为二进制，数字相加
    let num = 0;
    if(mask){
        let arrMask = mask.split(".");
        for(let i = 0; i<arrMask.length; i++){
            let arrConvert = Number(arrMask[i]).toString(2).split("");
            for(let j = 0; j < arrConvert.length; j++){
                num = num + Number(arrConvert[j])
            }
        }
    }

    var netWorkTopic = new ROSLIB.Topic({
        ros: NAV.ros,
        name: withNs('/shell_string'),
        messageType: 'std_msgs/String'
    });

    options['mode'] = PARAMS.NetworkMode.wifi;
    // var optionStr = JSON.stringify(options);
    var optionStr = checked ? `~/catkin_ws/www/shell/comm.sh -m wifi -s ${ssid} -p ${password} -i ${ip} -k ${num} -g ${geteway} -a` : `~/catkin_ws/www/shell/comm.sh -m wifi -s ${ssid} -p ${password} -i ${ip} -k ${num} -g ${geteway}`
    var msg = new ROSLIB.Message({
        data: optionStr
    });
    netWorkTopic.publish(msg);
    var storage = window.localStorage;
    storage.setItem('wifiInfos',JSON.stringify(options));
    store.commit('switchWifiSetting',false);
    DATA.loading = '连接中，正在切换'
    setTimeout(function(){
        window.location.href = `http://${ip}`;
    }, 10000);
};

/**
 * wifi复位
 * **/
export function wifiRest(options){
    var netWorkTopic = new ROSLIB.Topic({
        ros: NAV.ros,
        name: withNs('/shell_string'),
        messageType: 'std_msgs/String'
    });
    options['mode'] = PARAMS.NetworkMode.ap;
    // var optionStr = JSON.stringify(options);
    var optionStr = `~/catkin_ws/www/shell/comm.sh -m ap`;
    var msg = new ROSLIB.Message({
        data: optionStr
    });
    netWorkTopic.publish(msg);
    store.commit('switchWifiSetting',false);
    // reconnect
    DATA.loading = '连接中，正在切换'
    setTimeout(function(){
        window.location.href = `http://10.42.0.1`;
    }, 3000);
    store.commit('switchWifiSetting',false);
};




/**
 * 检索ip是否匹配
 * **/
function checkIp(ipStr){
	//match字符串内检索指定的值
    ipStr = ipStr.match(/(\d+)\.(\d+)\.(\d+)\.(\d+)/g);
    if (ipStr == null) 
    {
        return false;
	} 
	//正则匹配的第几个子匹配字符串
    else if (RegExp.$1 > 255 || RegExp.$2 > 255 || RegExp.$3 > 255 || RegExp.$4 > 255) 
    {
        return false;
    } 
    else 
    {
        return true;
    }
}

/**
 * 点击地图隐藏其他模态框
 * type:相对应的不处理
 * **/
export function hideOtherModule(type){
    switch (type){
        default:
        store.commit('switchAutherModule',false)
        store.commit('switchMapList',false);
        store.commit('switchDelMap',false);
        store.commit('switchMoreBtns',false);
        store.commit('switchDragModule',false);
        store.commit('switchWifiSetting',false);
        store.commit('switchAddStation',false);
        store.commit('switchStationList',false);
        store.commit('switchTrackList',false);
        store.commit('switchAddMap',false);
        store.commit('switchStationModule',false);
        store.commit('switchDeleteStation',false);
        store.commit('switchStationEdit',false);
        store.commit('switchDelMap',false);
        store.commit('switchTrackModule',false);
        store.commit('switchDeleteTrack',false);
        store.commit('switchAddTrack',false);
        store.commit('switchShowMask',false);
        store.commit('switchUpdate',false);
        store.commit('switchVoice',false);
        store.commit('switchLocationEstimate', false);
    }
}